package uk.ac.cam.cl.projectcharlie2010.physicalboard;

import uk.ac.cam.cl.projectcharlie2010.hardwaremove.RS232Example;
import ch.nostromo.tiffanys.game.board.Board;
import ch.nostromo.tiffanys.game.move.Move;

public class PhysicalMoveCalculator implements PhysicalBoardController
{
	// ---------- Static ---------- //
	// Desired level of logging
	// -1: none
	// 0: setup
	// 1: pieces
	// 2: xy co-ordinates
	// 3: angles
	// 4: motor steps
	public static int LOG_LEVEL = -1;
	// Approx number of squares each segment of a straight line will be.
	private static final double RESOLUTION = 1.0;
	// Flag indicating whether to attempt outputting to serial.
	private static final boolean SERIAL = true;
	// Serial connection object.
	private static RS232Example serialConnection = null;
	private static final int arm1CorrectionFactor = 10;
	private static final int arm2CorrectionFactor = 6;
	
	public static void log(String s, int level) {
		if (level <= LOG_LEVEL) {
			for (int i = 0; i < level; ++i) {
				System.out.print("\t");
			}
			System.out.println(s);
		}
	}
	
	// ---------- Object ---------- //
	// Variable prefixes:
	// - a: angle
	// - i: index
	// - l: length
	// - n: number of
	// - p: chess piece
	// - x: x co-ordinate
	// - y: y co-ordinate
	
	// Current position of the arm.
	private Double xCurr = null;
	private Double yCurr = null;
	private double ax1Curr = 0.0;
	private double a12Curr = 0.0;
	private boolean servoCurr = false;
	// True indicates left side.
	private boolean sideCurr = true;
	private String[] FEN;
	private PhysicalBoardControllerListener boardListener;
	
	/**
	 * @param boardListener
	 *            The callback object.
	 * @param comPort
	 *            The Serial Port to be used for communicating with the hardware.
	 */
	public PhysicalMoveCalculator(PhysicalBoardControllerListener boardListener, String comPort) {
		if (serialConnection == null && SERIAL == true) {
			serialConnection = new RS232Example();
			try {
				serialConnection.connect(comPort);
			} catch (Exception e) {
				System.err.println("Fatal error: Cannot connect over serial connection.");
				e.printStackTrace();
				System.exit(1);
			}
		}
		
		this.boardListener = boardListener;
	}
	
	/**
	 * Executes a chess move using the arm.
	 * 
	 * @param move
	 *            A {@link Move} object containing the move to be executed.
	 * @param FENString
	 *            A string containing the board state, from before the move, in Forsyth-Edwards notation.
	 * @see <a href="http://en.wikipedia.org/wiki/Forsyth-Edwards_Notation">Wikipedia article on FEN</a>
	 */
	public void executePhysicalMove(Move move, String FENString) {
		
		int xFrom, yFrom, xTo, yTo;
		boolean whiteMoving, enPassant, knight;
		
		// Interpret from/to strings
		// (0,0) is at h8; (7,7) is at a1.
		xFrom = 7 - (move.getFrom().charAt(0) - 97);
		yFrom = 7 - (move.getFrom().charAt(1) - 49);
		xTo = 7 - (move.getTo().charAt(0) - 97);
		yTo = 7 - (move.getTo().charAt(1) - 49);
		
		// Initialise boolean flags
		FEN = FENString.split(" ");
		whiteMoving = FEN[1].equals("w");
		enPassant = ((move.getStartingPiece() & Board.PIECE_PAWN) == Board.PIECE_PAWN) && FEN[3].equals(move.getTo());
		knight = (move.getStartingPiece() & Board.PIECE_KNIGHT) == Board.PIECE_KNIGHT;
		
		log("PhysicalMoveCalculator(" + xFrom + "," + yFrom + " -> " + xTo + "," + yTo + (whiteMoving ? " ; whiteMoving" : " ; blackMoving")
				+ (enPassant ? " ; enPassant" : "") + (knight ? " ; knight" : "") + (move.isStrikeMove() ? " ; strike" : "")
				+ (move.isCastlingLong() ? " ; q-s castle" : (move.isCastlingShort() ? " ; k-s castle" : "")) + ")\n", 0);
		
		// Break Move object into one or two calls to movePiece();
		if (move.isPromotion()) {
			// TODO implement promotion handling
		} else if (move.isStrikeMove()) {
			takePiece(xTo, yTo);
			movePiece(xFrom, yFrom, xTo, yTo, knight);
		} else if (move.isCastlingLong()) {
			int yTemp = (whiteMoving ? 7 : 0);
			movePiece(3, yTemp, 5, yTemp, false);
			movePiece(7, yTemp, 4, yTemp, true);
		} else if (move.isCastlingShort()) {
			int yTemp = (whiteMoving ? 7 : 0);
			movePiece(3, yTemp, 1, yTemp, false);
			movePiece(0, yTemp, 2, yTemp, true);
		} else if (enPassant) {
			takePiece(xTo, yTo + (yTo < 4 ? 1 : -1));
			movePiece(xFrom, yFrom, xTo, yTo, false);
		} else {
			movePiece(xFrom, yFrom, xTo, yTo, knight);
		}
		
		log("Return to origin", 2);
		moveAngleViaFolded(0.0, 0.0);
		sideCurr = true;
		moveAngle(-5, -5);
		
		xCurr = null;
		yCurr = null;
		
		boardListener.executePhysicalMoveFinished();
	}
	
	/**
	 * Removes a piece from the board and places in the relevant location on the side.
	 * 
	 * @param x
	 *            Zero-based index denoting the file of the piece to be removed.
	 * @param y
	 *            Zero-based index denoting the rank of the piece to be removed.
	 */
	public void takePiece(int x, int y) {
		log("takePiece(" + x + "," + y + ")", 1);
		int xTo = 0;
		int yTo = 0;
		int iFEN = 0;
		
		// Find piece taken
		for (int i = 0; i < y; ++i) {
			iFEN = FEN[0].indexOf("/", iFEN) + 1;
		}
		for (int i = 0; i < 7 - x; ++iFEN) {
			char c = FEN[0].charAt(iFEN);
			if (c >= 49 && c <= 57) {
				i += (c - 48);
			} else {
				++i;
			}
		}
		char pTaken = FEN[0].charAt(iFEN);
		
		// Find the number of pieces, the same as pTaken, which have not yet been taken.
		int npTaken = 0;
		for (char c : FEN[0].toCharArray()) {
			if (c == pTaken) {
				++npTaken;
			}
		}
		
		// Test which side taken
		boolean white = false;
		if (pTaken > 64 && pTaken < 91) {
			white = true;
			pTaken += 32;
		}
		
		if (pTaken == 'k') {
			xTo = 3;
			yTo = (white ? 8 : -1);
			movePiece(x, y, xTo, yTo, true);
		} else {
			// Calculate yTo
			switch (pTaken) {
			case 'p':
				xTo = 1;
				yTo = 8 - npTaken;
				break;
			case 'r':
				yTo = (npTaken == 2 ? 0 : 6);
				break;
			case 'n':
				yTo = (npTaken == 2 ? 1 : 5);
				break;
			case 'b':
				yTo = (npTaken == 2 ? 2 : 4);
				break;
			case 'q':
				yTo = 3;
				break;
			}
			log("pTaken: " + pTaken + ", hence y: " + yTo, 1);
			
			// Calculate xTo
			if (white) {
				log("takePiece: white", 1);
				xTo = 9 - xTo;
			} else {
				log("takePiece: black", 1);
				xTo = -2 + xTo;
			}
			
			int distTargetSide = (white ? 7 - x : x);
			// Move to calculated space via nearest edge of board, excluding that opposite the target edge.
			int minToSide = Math.min(distTargetSide, Math.min(y, 7 - y));
			if (minToSide == distTargetSide) {
				movePiece(x, y, xTo, yTo, true);
			} else {
				int tempRank;
				if (minToSide == y) {
					tempRank = -1;
				} else {
					tempRank = 8;
				}
				movePiece(x, y, x, tempRank, true);
				moveServo(true);
				int targetSide = (white ? 7 : 0);
				moveArmStraight(targetSide, tempRank);
				movePiece(targetSide, tempRank, xTo, yTo, true);
			}
		}
	}
	
	/**
	 * Executes the movement of a piece.
	 * 
	 * @param xFrom
	 *            Zero-based index denoting the starting file.
	 * @param yFrom
	 *            Zero-based index denoting the starting rank.
	 * @param xTo
	 *            Zero-based index denoting the destination file.
	 * @param yTo
	 *            Zero-based index denoting the destination rank.
	 * @param edge
	 *            If true this indicates that the piece must be moved along the edge of squares only. This is used for when the direct, straight-line
	 *            path might not be clear e.g. a knight's move or the removal of a piece that has been taken.
	 */
	public void movePiece(int xFrom, int yFrom, int xTo, int yTo, boolean edge) {
		log("movePiece(" + xFrom + "," + yFrom + " -> " + xTo + "," + yTo + (edge ? " ; edge" : "") + ")", 1);
		moveArm(xFrom, yFrom);
		
		moveServo(true);
		
		if (edge) {
			double xTemp1, yTemp1, xTemp2, yTemp2;
			// Move to corner of current square
			xTemp1 = xFrom + (xTo > xFrom ? 0.5 : -0.5);
			yTemp1 = yFrom + (yTo > yFrom ? 0.5 : -0.5);
			moveArmStraight(xTemp1, yTemp1);
			// Move along X
			xTemp2 = xTo + (xTo < xFrom ? 0.5 : -0.5);
			if (xTemp2 != xTemp1) {
				moveArmStraight(xTemp2, yTemp1);
			}
			// Move along Y
			yTemp2 = yTo + (yTo < yFrom ? 0.5 : -0.5);
			if (yTemp2 != yTemp1) {
				moveArmStraight(xTemp2, yTemp2);
			}
			// Move to centre of destination square
			moveArmStraight(xTo, yTo);
			
		} else {
			moveArmStraight(xTo, yTo);
		}
		
		moveServo(false);
	}
	
	/**
	 * Moves the arm in a manner approximating a straight line.
	 * 
	 * @param xTo
	 *            Zero-based index denoting the destination file, including fractions.
	 * @param yTo
	 *            Zero-based index denoting the destination rank, including fractions.
	 */
	@SuppressWarnings("boxing")
	public void moveArmStraight(double xTo, double yTo) {
		log("moveArmStraight(" + xTo + "," + yTo + ")", 2);
		
		double xDiff = xTo - xCurr;
		double yDiff = yTo - yCurr;
		
		double xyDiff = Math.sqrt(xDiff * xDiff + yDiff * yDiff);
		
		double nSegments = Math.floor(xyDiff / RESOLUTION) + 1.0;
		double xSegment = xDiff / nSegments;
		double ySegment = yDiff / nSegments;
		
		double xTemp = xCurr;
		double yTemp = yCurr;
		for (int i = 0; i < nSegments; ++i) {
			xTemp += xSegment;
			yTemp += ySegment;
			moveSegment(xTemp, yTemp);
		}
	}
	
	/**
	 * Moves the arm directly. Unlike {@link moveArm} this does not attempt to prevent collisions with the side of the board.
	 * 
	 * @param xTo
	 *            Zero-based index denoting the destination file, including fractions.
	 * @param yTo
	 *            Zero-based index denoting the destination rank, including fractions.
	 */
	@SuppressWarnings("boxing")
	public void moveSegment(double xTo, double yTo) {
		// Assumption that a segment is always small enough that the start point and end point do not *require* the arm to be in different
		// orientations.
		
		log("moveSegment(" + xTo + "," + yTo + ")", 2);
		double[][] aa = aaFromXY(xTo, yTo);
		int side = sideCurr ? 0 : 1;
		
		if (aa[side][0] < 0.0 || aa[side][0] > Math.PI) {
			sideCurr = !sideCurr;
			side = 1 - side;
			moveServo(false);
			moveArm(xCurr, yCurr);
			moveServo(true);
		}
		
		moveAngle(aa[side][0], aa[side][1]);
		xCurr = xTo;
		yCurr = yTo;
	}
	
	/**
	 * Moves the arm as directly as possible. Will attempt to prevent collisions with the side of the board.
	 * 
	 * @param xTo
	 *            Zero-based index denoting the destination file, including fractions.
	 * @param yTo
	 *            Zero-based index denoting the destination rank, including fractions.
	 */
	public void moveArm(double xTo, double yTo) {
		log("moveArm(" + xTo + "," + yTo + ")", 2);
		double[][] aa = aaFromXY(xTo, yTo);
		int side = sideCurr ? 0 : 1;
		
		if (aa[side][0] < 0.1 || aa[side][0] > Math.PI) {
			sideCurr = !sideCurr;
			side = 1 - side;
		}
		
		moveAngleViaFolded(aa[side][0], aa[side][1]);
		xCurr = xTo;
		yCurr = yTo;
	}
	
	/**
	 * Calculates the two sets of angles that will place the arm in the desired location. The return array contains two sub-arrays; the first is the
	 * set of angles for when the arm is in the left orientation, the second set for the right orientation. Each sub-array contains first a1x then
	 * a12.
	 * 
	 * @param x
	 *            Zero-based index denoting the destination file, including fractions.
	 * @param y
	 *            Zero-based index denoting the destination rank, including fractions.
	 */
	public double[][] aaFromXY(double x, double y) {
		log("aaFromXY(" + x + "," + y + ")", 3);
		/*
		 * Define a triangle with side '1', the segment of the arm closer to the motor; side '2', the segment of the arm further from the motor; and
		 * side '3', the imaginary line closing the triangle. Define the origin to be at the motor with axes parallel to those used earlier.
		 */
		x = x * 54.0 - 189.0;
		y = y * 54.0 + 101.0;
		
		double l1 = 294.0;
		double l2 = 260.0;
		double l3 = Math.sqrt(x * x + y * y);
		
		// cosA = (b^2 + c^2 - a^2) / (2*b*c)
		double a13 = Math.acos((l1 * l1 + l3 * l3 - l2 * l2) / (2.0 * l1 * l3));
		double ax3 = Math.atan2(y, x);
		double a12 = Math.acos((l1 * l1 + l2 * l2 - l3 * l3) / (2.0 * l1 * l2));
		
		log("aaFromXY: { { " + (ax3 - a13) + ", " + (a12) + " }, { " + (ax3 + a13) + ", " + (-a12) + " } }", 3);
		double[][] aa = { { ax3 - a13, a12 }, { ax3 + a13, -a12 } };
		return aa;
	}
	
	/**
	 * Moves the servo to enable or disable moving of pieces along with the arm.
	 * 
	 * @param raise
	 *            Whether to raise the servo, enabling movement of pieces, or lower the servo, disabling movement of pieces.
	 */
	public void moveServo(boolean raise) {
		log("moveServo(" + raise + ")", 2);
		servoCurr = raise;
	}
	
	/**
	 * Moves the arm in such a way that it will not hit the side. This is implemented by always moving arm1 to the perpendicular position.
	 * 
	 * @param ax1
	 *            Desired angle in radians between the x axis and arm 1, positive in the anti-clockwise direction.
	 * @param a12
	 *            Desired angle in radians between arm 1 and arm 2, positive in the clockwise direction.
	 */
	private void moveAngleViaFolded(double ax1, double a12) {
		if (ax1 == ax1Curr && a12 == a12Curr) {
			return;
		}
		log("moveAngleViaFolded(" + ax1 + "," + a12 + ")", 3);
		if ((ax1 <= 0.0 && a12 <= 0.0 && a12Curr > 0.0) || (ax1Curr <= 0.0 && a12Curr <= 0.0 && a12 > 0.0)) {
			moveAngle(ax1, a12);
			return;
		}
		
		// Move arm 1 to rightAngle and arm 2 to either 0.0 or a12
		double rightAngle = Math.PI / 2.0;
		if (ax1Curr < rightAngle && a12Curr > 0) {
			a12Curr = Math.max(0.0, a12);
		} else if (ax1Curr > rightAngle && a12Curr < 0) {
			a12Curr = Math.min(0.0, a12);
		}
		moveAngle(rightAngle, a12Curr);
		
		if (ax1 < rightAngle) {
			// Ensure arm 2 is on the correct side of -rightAngle and 0.0
			if (a12 < -rightAngle) {
				if (a12Curr > -rightAngle) {
					moveAngle(rightAngle, -rightAngle);
				}
			} else if (a12 < 0.0) {
				if (a12Curr < -rightAngle) {
					moveAngle(rightAngle, -rightAngle);
				} else if (a12Curr > 0.0) {
					moveAngle(rightAngle, 0.0);
				}
			} else {
				if (a12Curr < 0.0) {
					moveAngle(rightAngle, 0.0);
				}
			}
		} else {
			// Ensure arm 2 is on the correct side of rightAngle and 0.0
			if (a12 > rightAngle) {
				if (a12Curr < rightAngle) {
					moveAngle(rightAngle, rightAngle);
				}
			} else if (a12 > 0.0) {
				if (a12Curr > rightAngle) {
					moveAngle(rightAngle, rightAngle);
				} else if (a12Curr < 0.0) {
					moveAngle(rightAngle, 0.0);
				}
			} else {
				if (a12Curr > 0.0) {
					moveAngle(rightAngle, 0.0);
				}
			}
		}
		
		moveAngle(ax1, a12);
	}
	
	/**
	 * Changes angles into the steps used by the motors and sends these to the serialConnection.
	 * 
	 * @param ax1
	 *            Desired angle in radians between the x axis and arm 1, positive in the anti-clockwise direction.
	 * @param a12
	 *            Desired angle in radians between arm 1 and arm 2, positive in the clockwise direction.
	 */
	private void moveAngle(double ax1, double a12) {
		if (ax1 == ax1Curr && a12 == a12Curr) {
			return;
		}
		// Motor positions are centred at 5000 with 2400 steps per revolution for motor 1 and 1200 for motor 2.
		log("moveAngle(" + ax1 + "," + a12 + ")", 3);
		int arm1Position = (int) Math.round(5000.0 - (1200.0 * ax1 / Math.PI));
		int arm2Position = (int) Math.round(5000.0 - (600.0 * a12 / Math.PI));
		log("sendMove(" + arm1Position + "," + arm2Position + "," + servoCurr + ")", 4);
		if (SERIAL == true) {
			if (arm1Position != 0 && arm2Position != 0) {
				serialConnection.sendMove(arm1Position + arm1CorrectionFactor, arm2Position + arm2CorrectionFactor, servoCurr);
			}
		}
		ax1Curr = ax1;
		a12Curr = a12;
	}
}
